/*
Wally - the sensor turret. 

PINs in use:


A0	: IR sensor (in)
A4	: I2C communication
A5	: I2C comunication

0	: servo control (out)
2	: RX pin for XBee shield
3	: TX pin for Xbee shield
7	: PING))) ultrasound sensor (in)
4	: sound sensor (in)
13	: Laser (high to switch on)

*/
#include <Servo.h>					// include the standard servo library
#include <SoftwareSerial.h>			// include software serial library for Xbee shield communication on digital pins 2 and 3

#include "wally.h"
#include "i2c_ peripherals.h"

Servo sensorTurret;					// set a variable to map the servo

uint16_t turretAngle  =  0;			// set a variable to store the servo position
uint16_t index  	  =	 0;			// the index of the current reading

const uint8_t IRpin   = A0;			// analog pin A0 for reading the IR sensor
									// A4 and A5 reserved for I2C communication

const uint8_t servoPin=  0;			// digital pin 0 for controlling servo position

const uint8_t xbeeRX =	 2;			// digital pin 2 for Xbee UART receive
const uint8_t xbeeTX =	 3;			// digital pin 3 for Xbee UART transmit

const uint8_t soundPin=  4;			// digital pin 4 for reading the sound impact sensor (CANNOT USE HW INTERRUPT!)

const uint8_t PINGpin =  7;			// digital pin 7 for reading the PING))) sensor

const uint8_t laserPin= 13;			// digital pin 13 to turn on/off laser

const uint8_t SERVOmax= 165;		// maximum degrees turn for servo

const uint8_t sensorPING= 0;
const uint8_t sensorIR =  1;
const uint8_t sensorSound=2;

boolean DETECTED 		=  0;				// have we found someone?
boolean HEARD	  		 =  0;			// have we heard something?

const boolean REPORT   =  0;		// Report readings for external processing (not compatible with DEBUG=1)
const boolean DEBUG    =  1;		// Increase reporting

boolean interruptDisable = 0;

SoftwareSerial xbeeSerial(xbeeRX, xbeeTX);		// initialize software serial port on pins 2 and 3 for communication with XBee shield


void setup()
{
	//attachInterrupt(1, readSensorSound, FALLING);	// 1 means pin 3 (predefined) ///  WARNING: BROKEN. XBEE SHIELD USES THESE PINS

	sensorTurret.attach(servoPin);					// attach servo to our servoPin
	pinMode(soundPin, INPUT);      					// sets the digital pin 7 as input
	pinMode(laserPin, OUTPUT);
	digitalWrite(laserPin, LOW);					// turn laser off


	Serial.begin(115200);							// initialize the serial port
	xbeeSerial.begin(9600);						// sets software serial speed to 115.2k - must match Xbee hardware config
	Wire.begin();									// masters can but doesn't have to have an address
	//setRTCtime();									// ONLY enable this if we want to RESET the clock
}

void loop()
{
char incomingByte;
delay(3999);

digitalWrite(laserPin, HIGH);					// turn laser on
//xbeeSerial.print("{s1:c0:RUNE}");	// testing
while(0) {

	if(xbeeSerial.available()) incomingByte = xbeeSerial.read();
	Serial.print(" I got: \"");
	Serial.print(incomingByte);
	Serial.println("\"");
	delay(100);
	}


//sensorSweep();

/*
	///////////////////////////////////////////////////////////////////////////////////////////////
	//	TESTING
	//while(1) { getRTCtime(); }
	uint16_t distance;

	//Serial.println(F("*** Starting new sensor run ***"));
	distance = readSensor(0);
	Serial.print(F(" *** PING))) : "));
	Serial.println(distance);

	distance = readSensor(1);
	Serial.print(F(" *** Sharp   : "));
	Serial.println(distance);

	delay(500);
	///////////////////////////////////////////////////////////////////////////////////////////////

*/

}


/* begin rotating the servo and getting sensor values */
void sensorSweep(void)
{

	digitalWrite(laserPin, HIGH);		// enable laser to indicate sweep
	const uint8_t numReadings 	= 5;	// set a variable for the number of readings to take per position. Original: 10
	uint8_t currentSensor = sensorIR;
	float total 	= 0;				// the total of all readings must be a float to allow totaling of float values
	uint16_t average = 0;				// the average


	for(turretAngle = SERVOmax; turretAngle > 0; turretAngle--)	// going right to right.
	{
		sensorTurret.write(turretAngle);

      	for (index = 0; index<=numReadings;index++)			// take x number of readings from the sensor and average them - evens out the spikes
        	total = total + readSensor(currentSensor);

    	average = (int) total/numReadings;					// create average reading CAST TO INT!! remove the decimal places

		if (index >= numReadings)  							// reset the counts when at the last item of the array
		{
			index = 0;
			total = 0;
		}
		if(REPORT)
		{
			Serial.print("X");								// print leading X to mark the following value as degrees
			Serial.print(turretAngle);						// current servo position
			Serial.print("V");								// preceeding character to separate values
			Serial.println(average);						// average of sensor readings
		}
    	// done with one position, moving on to the next...
	}

  //start going right to left after we got to 180 degrees - same code as above
	for(turretAngle = 0; turretAngle < SERVOmax; turretAngle++)	// going left to left
	{
		sensorTurret.write(turretAngle);

//		for (index = 0; index<=numReadings;index++)			// take x number of readings from the sensor and average them - evens out the spikes
//        	total = total + readSensor(currentSensor);

// 		average = (int) total/numReadings;
 		if (index >= numReadings)
 		{
 			index = 0;
 			total = 0;
 		}
		if(REPORT)
		{
			Serial.print("X");
			Serial.print(turretAngle);
			Serial.print("V");
			Serial.println(average);
		}
   }
}


uint16_t readSensor(uint8_t sensor)
{
	float distance;
	switch(sensor)
	{
		/*
			Parallax PING))) is a very accurate binary distance sensor, but has a wide field of view.
			For unknown reason it's consistently 5cm short. I've manually compensated for that in the code
			Distance range: 20cm - 300cm
		*/
		case sensorPING:
		{
		long duration;

		// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
		// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
		//if(DEBUG) Serial.print(F("Triggering pulse on PING))) sensor..."));
		pinMode(PINGpin, OUTPUT);
		digitalWrite(PINGpin, LOW);
		delayMicroseconds(2);
		digitalWrite(PINGpin, HIGH);
		delayMicroseconds(5);
		digitalWrite(PINGpin, LOW);
		//if(DEBUG) Serial.println(F("DONE"));

		/*
			The same pin is used to read the signal from the PING))):
			a HIGH pulse whose duration is the time (in microseconds) from the sending of the ping to the
			reception of its echo off of an object.
		*/
		pinMode(PINGpin, INPUT);
		duration = pulseIn(PINGpin, HIGH);

		//uint16_t raw_reading = duration;
		//if (DEBUG) Serial.print(F("PING))) reading: "));			// Debug debug
		//if (DEBUG) Serial.println(raw_reading);						// Debug debug

		// Convert the time into a distance. The speed of sound is 340 m/s or 29 microseconds per centimeter.
		// The ping travels out and back, so to find the distance of the object we take half of the distance travelled.
		distance = (duration / 29 / 2) + 5;	// Comes up 5cm short, no idea why. Manually fixed here
		//if (DEBUG) Serial.print(F("PING))) distance: "));			// Debug debug
		//if (DEBUG) Serial.println(distance);						// Debug debug

		break;
		}

		/*
			Sharp Rangefinder (GP2Y0A02YK0F) is a slightly less accurate analog distance monitor, but has a
			very narrow field of view.
			Distance range: 30cm - 180cm
		*/

		case sensorIR:
		{
			RunningMedian samples = RunningMedian();	// We now an object to get median value of samples
			//float total = 0;
			uint8_t iterations = 5;
			for (uint8_t i = 0; i < iterations; i++)	// Find the median from several readings
			{
				samples.add(analogRead(IRpin));
			}
				distance = 10650.08 * pow(samples.getMedian(), -0.950) - 10;	// Tweaked for my IR sensor

				//total += distance;

			//distance = total / iterations;
			//if (DEBUG) Serial.print(F("Sharp Range Finder distance : "));
			//if (DEBUG) Serial.println(distance);

			break;
		}
	}
	delay(50);													// Give the sensors time to catch their breath
	return((int)distance);

}

void readSensorSound(void)
{
	// Note: the input pulse from the sensor should be about 50ms
	if (interruptDisable) return;
	Serial.println(F("WE HEARD SOMETHING!"));
	HEARD = 1;
}